#include <iostream>
#include <string>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <istream>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>

//HQ add SNMP class
#include <net-snmp/net-snmp-config.h> //*****need to be changed in linux *******
#include <net-snmp/net-snmp-includes.h>//*****need to be changed in linux *******

//ASC INTERSECTION MIB
#define RED_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.2.1"                     //Object
#define YELLOW_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.3.1"
#define GREEN_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.4.1"
#define DONOTWALK_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.5.1"
#define PEDCLEAR_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.6.1"
#define WALK_GROUP "1.3.6.1.4.1.1206.4.2.1.1.4.1.7.1"
#define PREEMPTOR1 "1.3.6.1.4.1.1206.4.2.1.6.3.1.2.1"
#define PREEMPTOR2 "1.3.6.1.4.1.1206.4.2.1.6.3.1.2.2"
#define PREEMPTOR3 "1.3.6.1.4.1.1206.4.2.1.6.3.1.2.3"
#define PREEMPTOR4 "1.3.6.1.4.1.1206.4.2.1.6.3.1.2.4"

//RAMP METER MIB
#define	NUMBER_METEREDLANES "1.3.6.1.4.1.1206.4.2.2.3.1.2"
#define LANE1_MODE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.1.1"
#define LANE2_MODE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.1.2"
#define LANE3_MODE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.1.3"
#define LANE1_MANUALACTION "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.2.1"
#define LANE2_MANUALACTION "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.2.2"
#define LANE3_MANUALACTION "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.2.3"
#define LANE1_MANUALRATE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.4.1"
#define LANE2_MANUALRATE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.4.2"
#define LANE3_MANUALRATE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.4.3"
#define LANE1_DEMANDMODE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.18.1"
#define LANE2_DEMANDMODE "1.3.6.1.4.1.1206.4.2.2.3.1.7.1.18.2"
//emgerence green, set manual action to 5. undo, set to 3
//keep Red, set demand mode to 3. undo, set to 1
#define MAX_ITEMS 20

#define numPhases 8

//Qing add for phase status 121209
#define RED 0
#define GREEN 1
#define YELLOW 2

#define GIDNAMEPATH "/usr/local/bin/GIDinfo.txt"

char RAMP_RNDF[256]; //"/usr/local/bin/speedwayramp.rndf"//"/usr/local/bin/RAMP_METER.rndf"
char  INTERSECTION_RNDF[256]; //"/usr/local/bin/south67_phx.rndf"


//#define INTERSECTION_RNDF "/usr/local/bin/MountainSpeedway.rndf"//"INTERSECTION.rndf"
#define RAMP 0
#define INTERSECTION 1

// for socket communication with obu
#define PORT 5799 //GID port
#define BROADCAST_ADDR "192.168.255.255"
#define RSU_ADDR "192.168.1.21"

//Ramp meter ip port
const char *RMip = "150.135.152.24";
const char *RMport = "161";
//intersection asc controller ip port
const char *INTip = "150.135.152.23";
const char *INTport = "501";

//define log file name
char logfilename[256] = "/nojournal/log/rsugid_mprlog_";

char requestfilename[256] = "/usr/local/bin/requests.txt";

char rsuname[64]="Southern_67th_Phoenix";

int PhaseStatus[numPhases]={0};
void UpdateIntersectionStatus(int red, int yellow, int green);
void IntersectionPhaseRead();

//------------Time stamp------------------------------------------------
void     xTimeStamp( char * pc_TimeStamp_ )
{
          struct tm  * ps_Time;
          time_t       i_CurrentTime;
          char         ac_TmpStr[256];

          i_CurrentTime =  time(NULL);
          ps_Time = localtime( &i_CurrentTime );

                      //year
          sprintf(ac_TmpStr, "%d", ps_Time->tm_year + 1900);
          strcpy(pc_TimeStamp_, ac_TmpStr);

                      //month
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_mon + 1 );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //day
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_mday );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //hour
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_hour  );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //min
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_min );
          strcat(pc_TimeStamp_, ac_TmpStr);

                     //sec
          sprintf(ac_TmpStr, "_%d", ps_Time->tm_sec );
          strcat(pc_TimeStamp_, ac_TmpStr);
}


//end-------------------------------------------------------------------

char * getBufferAsBinaryString(void* in)
{
	int pos=0;
	char result;
	char bitstring[16];
	memset(bitstring, 0, 16);
	unsigned int *input= (unsigned int *)in;
	for(int i=7;i>=0;i--)
	{
		//if (((*input >> i) & 1)) result = '1';
		//else result = '0';
		if (i>0)
		{
			if (((*input >> i) % 2)) result = '1';
			else result = '0';
		}
		else
		{
			if (((*input) % 2)) result = '1';
			else result = '0';
		}

		bitstring[pos] = result;
		pos++;
	}
	bitstring[pos] = '1';
	strcat(bitstring,"END");
	return bitstring;

}

int msleep(unsigned long milisec)
{
    struct timespec req={0};
    time_t sec=(int)(milisec/1000);
    milisec=milisec-(sec*1000);
    req.tv_sec=sec;
    req.tv_nsec=milisec*1000000L;
    while(nanosleep(&req,&req)==-1)
       continue;
    return 1;
}

int outputlog(char *output)
{
	FILE * stream = fopen( logfilename, "r" );
        fseek( stream, 0L, SEEK_END );
        long endPos = ftell( stream );
        fclose( stream );

	std::fstream fs;
        if (endPos <1000000)
		fs.open(logfilename, std::ios::out | std::ios::app);
	else
		fs.open(logfilename, std::ios::out | std::ios::trunc);
	if (!fs || !fs.good())
	{
		std::cout << "could not open file!\n";
		return -1;
	}
	fs << output << std::endl;
        //fs << "Current File lenghth:"<<endPos<<std::endl;
	if (fs.fail())
	{
		std::cout << "failed to append to file!\n";
		return -1;
	}


	return 1;
}

int main ( int argc, char* argv[] )
{
	char temp[256];
	//Init log file
	//------log file name with Time stamp---------------------------
	char timestamp[128];
	xTimeStamp(timestamp);
	strcat(logfilename,timestamp);
	strcat(logfilename,".log");
	//------end log file name-----------------------------------

	//define broadcast network vars
  	int sockfd;
  	int broadcast=1;
  	struct sockaddr_in recvaddr;
  	struct sockaddr_in sendaddr;
  	int numbytes;
  	char sendMSG[256] ="";
	char RNDFstring[20480];//20k
	char Req_table[1024];
  	char recvbuff[256] = "";
  	int  recvbufflen = 256;

	unsigned int type = 0;

	std::fstream fs;
	fs.open(logfilename, std::ios::out | std::ios::trunc );

	if(argc > 1)

		if (strcmp(argv[1],"ramp")==0)
		{
			type = RAMP;
			printf("Run rsugid for ramp meter, start to broadcast GID...\n");
		}

		else if (strcmp(argv[1],"intersection")==0)
		{
			type = INTERSECTION;
			printf("Run rsugid for intersection, start to broadcast GID...\n");

		}
		else
		{
			printf("rsugid_mpr - Please select your facility type\n");
			printf("rsugid_mpr - Usage %s <ramp or intersection>\n", argv[0]);
			exit(0);
		}

	else

	{

	/*Use default hostname or IP*/
	printf("rsugid_mpr - Please select your facility type\n");
	printf("rsugid_mpr - Usage %s <ramp or intersection>\n", argv[0]);
	exit(0);

	}



        if((sockfd = socket(PF_INET,SOCK_DGRAM,0)) == -1)
        {
                perror("sockfd");
                exit(1);
        }

       if((setsockopt(sockfd,SOL_SOCKET,SO_BROADCAST,
                                &broadcast,sizeof broadcast)) == -1)
        {
                perror("setsockopt - SO_SOCKET ");
                exit(1);
        }
 //Network is unreachable
        sendaddr.sin_family = AF_INET;
        sendaddr.sin_port = htons(PORT);
        sendaddr.sin_addr.s_addr = INADDR_ANY;//inet_addr(RSU_ADDR);//INADDR_ANY;
        memset(sendaddr.sin_zero,'\0',sizeof sendaddr.sin_zero);

        if(bind(sockfd, (struct sockaddr*) &sendaddr, sizeof sendaddr) == -1)
        {
                perror("bind");
                exit(1);
        }
  //Read rndf file name
  	std::ifstream load_filename;
  	load_filename.open(GIDNAMEPATH);
  	int count = 0;
  	while (!load_filename.eof())
	{
		load_filename.getline(temp,64);
		if (count==0)
		{
			strcpy(INTERSECTION_RNDF,temp);
		}
		else if (count==1)
		{
			strcpy(RAMP_RNDF,temp);
		}
		count ++;

	 }

  	load_filename.close();


   //Read rndf once in a time
	std::ifstream load;
	if (type == RAMP) load.open(RAMP_RNDF);
	else if (type == INTERSECTION) load.open(INTERSECTION_RNDF);
	else {printf("\nwrong type!"); exit(0);}
	while (!load.eof())
	 {
	    load.getline(sendMSG,64);
	    strcat(RNDFstring,sendMSG);
	    strcat(RNDFstring,"\n");
	 }
	load.close();


  // HQ Change
        recvaddr.sin_family = AF_INET;
        recvaddr.sin_port = htons(PORT);
        recvaddr.sin_addr.s_addr = inet_addr(BROADCAST_ADDR) ; //INADDR_BROADCAST;
        memset(recvaddr.sin_zero,'\0',sizeof recvaddr.sin_zero);

	int addr_length = sizeof ( recvaddr );
	while ( true )

	{
		std::cout << "\n Sending RNDF data to OBU.\n";



		numbytes = sendto(sockfd, RNDFstring,strlen(RNDFstring)+1 , 0,(struct sockaddr *)&recvaddr, addr_length);
		if (numbytes>0)
   		{
              		printf("broadcast a packet. length= %d \r\n",numbytes);

		}
        else
        {
			perror("sendto");
			outputlog("Failed to send GID !");
		}

		msleep(500);


		if (type == INTERSECTION)
		{
			//read signal status

			IntersectionPhaseRead();

			sprintf(sendMSG,"Signal_status %s",rsuname);
			char temp[64];
			for (int i=0;i<numPhases;i++)
			{
				sprintf(temp," %d",PhaseStatus[i]);
				strcat(sendMSG,temp);
			}
			printf("\n Current Signal:%s \n",sendMSG);

			numbytes = sendto(sockfd, sendMSG,strlen(sendMSG)+1 , 0,(struct sockaddr *)&recvaddr, addr_length);
			if (numbytes>0)
			{
    	  		printf("broadcast a packet. length= %d \r\n",numbytes);

			}
			else
			{
				perror("sendto");
				outputlog("Failed to send Signal status !");
			}

			msleep(500);
			//read request table

			std::ifstream load;
			load.open(requestfilename);
			strcpy(Req_table,"");
			int Num_request = -1;
			int Count = 0;
			while (!load.eof())
			 {
			    load.getline(temp,64);
			    if (Count==0)
			    {
					char aa[64];
					sscanf(temp,"%s%d",aa,&Num_request);

				}
			    strcat(Req_table,temp);
			    strcat(Req_table,"\n");
			    Count++;
			 }
			load.close();

			if (Num_request>=0)
			{

				numbytes = sendto(sockfd, Req_table,strlen(Req_table)+1 , 0,(struct sockaddr *)&recvaddr, addr_length);
				if (numbytes>0)
				{
				   	printf("broadcast a packet. length= %d \r\n",numbytes);
				   	printf("\n Current requests:%s \n",Req_table);

				}
				else
				{
					perror("sendto");
					outputlog("Failed to send request table !");
				}
			}



		}//if type

	}

  close(sockfd);

  return 0;
}


void IntersectionPhaseRead()
{

	netsnmp_session session, *ss;
			netsnmp_pdu *pdu;
			netsnmp_pdu *response;

			oid anOID[MAX_OID_LEN];
			size_t anOID_len;

			netsnmp_variable_list *vars;
			int status;
			int count=1;



			/*
			* Initialize the SNMP library
			*/
			init_snmp("RSU");

			/*
			* Initialize a "session" that defines who we're going to talk to
			*/
			snmp_sess_init( &session );                   /* set up defaults */
			//char *ip = m_rampmeterip.GetBuffer(m_rampmeterip.GetLength());
			//char *port = m_rampmeterport.GetBuffer(m_rampmeterport.GetLength());
			char ipwithport[64];
			strcpy(ipwithport,INTip);
			//strcat(ipwithport,":");
			//strcat(ipwithport,INTport); //for ASC get status, DO NOT USE port!!!
			session.peername = strdup(ipwithport);
			/* set the SNMP version number */
			session.version = SNMP_VERSION_2c; //for ASC intersection
			//session.version = SNMP_VERSION_1; //for Rampmeter

			/* set the SNMPv1 community name used for authentication */
			session.community = (u_char *)"public";
			session.community_len = strlen((const char *)session.community);

			SOCK_STARTUP;
			ss = snmp_open(&session);                     /* establish the session */

			if (!ss)
			{
				snmp_sess_perror("RSU", &session);
				SOCK_CLEANUP;
				exit(1);
			}

			/*
			* Create the PDU for the data for our request.
			*   1) We're going to GET the system.sysDescr.0 node.
			*/
			pdu = snmp_pdu_create(SNMP_MSG_GET);
			anOID_len = MAX_OID_LEN;
			//for INTERSECTION CONTROLLER

			if (!snmp_parse_oid(RED_GROUP, anOID, &anOID_len)) //get RED group
			{
				snmp_perror(RED_GROUP);
				SOCK_CLEANUP;
				exit(1);
			}
			snmp_add_null_var(pdu, anOID, anOID_len);

			if (!snmp_parse_oid(YELLOW_GROUP, anOID, &anOID_len)) //get Yellow group
			{
				snmp_perror(YELLOW_GROUP);
				SOCK_CLEANUP;
				exit(1);
			}
			snmp_add_null_var(pdu, anOID, anOID_len);

			if (!snmp_parse_oid(GREEN_GROUP, anOID, &anOID_len)) //get Green group
			{
				snmp_perror(GREEN_GROUP);
				SOCK_CLEANUP;
				exit(1);
			}
			snmp_add_null_var(pdu, anOID, anOID_len);

			/*
			* Send the Request out.
			*/
			status = snmp_synch_response(ss, pdu, &response);

			/*
			* Process the response.
			*/
			if (status == STAT_SUCCESS && response->errstat == SNMP_ERR_NOERROR)
			{
				/*
				* SUCCESS: Print the result variables
				*/
				int *out = new int[MAX_ITEMS];
				int i =0;
				for(vars = response->variables; vars; vars = vars->next_variable)
					print_variable(vars->name, vars->name_length, vars);

				/* manipuate the information ourselves */
				for(vars = response->variables; vars; vars = vars->next_variable)
				{
					if (vars->type == ASN_OCTET_STR)
					{
						char *sp = (char *)malloc(1 + vars->val_len);
						memcpy(sp, vars->val.string, vars->val_len);
						sp[vars->val_len] = '\0';
						//printf("value #%d is a string: %s\n", count++, sp);
						free(sp);
					}
					else
					{

						int *aa;
						aa =(int *)vars->val.integer;
						out[i++] = * aa;

						//printf("value #%d is NOT a string! Ack!. Value = %d \n", count++,*aa);


					}

				}
				//FOR ASC INTERSECTIONS_ Draw the lights
						UpdateIntersectionStatus(out[0],out[1],out[2]);
				//For RAMP METER
				//set ramp meter status
						//SetRampMeterStatus(out);



			} else {
				/*
				* FAILURE: print what went wrong!
				*/

				if (status == STAT_SUCCESS)
					fprintf(stderr, "Error in packet\nReason: %s\n",
					snmp_errstring(response->errstat));
				else if (status == STAT_TIMEOUT)
					fprintf(stderr, "Timeout: No response from %s.\n",
					session.peername);
				else
					snmp_sess_perror("snmpdemoapp", ss);

			}

			/*
			* Clean up:
			*  1) free the response.
			*  2) close the session.
			*/
			if (response)
				snmp_free_pdu(response);
			snmp_close(ss);

		SOCK_CLEANUP;


}


void UpdateIntersectionStatus(int red, int yellow, int green)
{
		char *sz_temp;
		char sz_red[16];
		char sz_yellow[16];
		char sz_green[16];
		//printf("red:%d, yellow:%d, green:%d \n",red,yellow,green);
		sz_temp = getBufferAsBinaryString(&red);
		strcpy(sz_red,sz_temp);
		//memcpy(sz_red,sz_temp,strlen(sz_temp));

		//printf("Red binary string:%s\n",sz_red);

		sz_temp=getBufferAsBinaryString(&yellow);
		strcpy(sz_yellow,sz_temp);
		//memcpy(sz_yellow,sz_temp,strlen(sz_temp));
		//printf("Yellow binary string:%s\n",sz_temp);

		sz_temp=getBufferAsBinaryString(&green);
		strcpy(sz_green,sz_temp);
		//memcpy(sz_green,sz_temp,strlen(sz_temp));
		//printf("Green binary string:%s\n",sz_temp);

		for(int i = numPhases - 1; i >= 0; i--)
			{
				bool isRed = false;
				bool isYellow = false;
				bool isGreen = false;

				if(sz_red[i] == '1')
				{
					isRed = true;

					PhaseStatus[numPhases - 1 - i] = RED;
				}
				if(sz_yellow[i] == '1')
				{
					isYellow = true;

					PhaseStatus[numPhases - 1 - i] = YELLOW;
				}
				if(sz_green[i] == '1')
				{
					isGreen = true;

					PhaseStatus[numPhases - 1 - i] = GREEN;
				}

				//Phases_New.isRed[numPhases - 1 - i] = isRed;
				//Phases_New.isYellow[numPhases - 1 - i] = isYellow;
				//Phases_New.isGreen[numPhases - 1 - i] = isGreen;

			}

}
